import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from xacro import process_file
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
     # 获取 Xacro 文件的完整路径
    xacro_file = os.path.join(
        get_package_share_directory('model'),
        'urdf',
        'robot.xacro'
    )

     # 生成 URDF 文件的路径
    package_share_dir = get_package_share_directory('model')
    urdf_output_dir = os.path.join(package_share_dir, 'urdf')
    os.makedirs(urdf_output_dir, exist_ok=True)
    urdf_file = os.path.join(urdf_output_dir, 'robot.urdf')

    # 处理 Xacro 文件生成 URDF 内容并保存到文件
    try:
        doc = process_file(xacro_file)
        robot_desc = doc.toprettyxml(indent='  ')
        with open(urdf_file, 'w') as f:
            f.write(robot_desc)
    except Exception as e:
        print(f"Error processing xacro file: {e}")
        return launch.LaunchDescription([])

    # 读取生成的 URDF 文件内容
    with open(urdf_file, 'r') as infp:
        robot_desc = infp.read()

    # 定义 Robot State Publisher 节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{'robot_description': robot_desc}]
    )

    # 获取 Gazebo ROS 包的共享目录
    gazebo_ros = get_package_share_directory('gazebo_ros')

    # 包含 Gazebo 启动文件
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(gazebo_ros, 'launch', 'gazebo.launch.py')
        ),
    )

    # 启动 spawn_entity 节点将机器人模型加载到 Gazebo 中
    spawn_entity = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'robot', '-topic', '/robot_description'],
        output='screen'
    )


    # 加载并激活 robot_joint_state_broadcaster 控制器
    load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
            'robot_joint_state_broadcaster'],
        output='screen'
    )

    # 加载并激活 robot_joint_state_broadcaster 控制器
    load_velocity_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
            'robot_velocity_controller'],
        output='screen'
    )

    return launch.LaunchDescription([
        robot_state_publisher_node,
        gazebo,
        spawn_entity,
         # 事件动作，当加载机器人结束后执行    
        launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=spawn_entity,
            on_exit=[load_joint_state_controller],)
        ),
        launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=load_joint_state_controller,
            on_exit=[load_velocity_controller],)
        ),
    ])